///\todo upgrade

//////////////////////////////////////////////////////////////////////////////////
//                                                                              //
//  This file is part of the buola project (https://code.google.com/p/buola/).  //
//                                                                              //
//  Copyright(c) 2007-2012 Xavi Gratal                                          //
//  gratal AT gmail DOT com                                                     //
//                                                                              //
//  Buola is free software: you can redistribute it and/or modify               //
//  it under the terms of the GNU General Public License as published by        //
//  the Free Software Foundation, either version 3 of the License, or           //
//  (at your option) any later version.                                         //
//                                                                              //
//  Buola is distributed in the hope that it will be useful,                    //
//  but WITHOUT ANY WARRANTY; without even the implied warranty of              //
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the               //
//  GNU General Public License for more details.                                //
//                                                                              //
//  You should have received a copy of the GNU General Public License           //
//  along with buola.  If not, see <http://www.gnu.org/licenses/>.              //
//                                                                              //
//////////////////////////////////////////////////////////////////////////////////

#ifndef __BUOLA_CV_COCTREEDEFAULTKEY_H__
#define __BUOLA_CV_COCTREEDEFAULTKEY_H__

#include <buola/mat.h>

namespace buola { namespace cvi {

class COcTreeDefaultKey
{
public:
    static const size_t cMaxDepth=15; //0 is the root. cMaxDepth are the leaves

private:
    static const int64_t cZero=int64_t(1)<<(cMaxDepth-1);
    static const int64_t cMaxValue=(int64_t(1)<<cMaxDepth)-1;
    static const uint64_t cMask=(int64_t(1)<<cMaxDepth)-1;
    static const uint64_t cInvalid=(uint64_t)-1;

    uint64_t Mask(int pDepth) const
    {
        uint64_t lMask=cMask>>(cMaxDepth-pDepth)<<(cMaxDepth-pDepth);
        return lMask+(lMask<<cMaxDepth)+(lMask<<2*cMaxDepth);
    }

    uint64_t CalcKey(const mat::CVec3f &pV,float pResolution) const
    {
        uint64_t lScaledX=uint64_t(int64_t(std::floor(pV.x()/pResolution))+cZero);
        if(lScaledX>cMaxValue) return cInvalid;
        uint64_t lScaledY=uint64_t(int64_t(std::floor(pV.y()/pResolution))+cZero);
        if(lScaledY>cMaxValue) return cInvalid;
        uint64_t lScaledZ=uint64_t(int64_t(std::floor(pV.z()/pResolution))+cZero);
        if(lScaledZ>cMaxValue) return cInvalid;

        return lScaledX+(lScaledY<<cMaxDepth)+(lScaledZ<<2*cMaxDepth);
    }

    uint64_t CalcKey(const mat::CVec3f &pV,float pResolution,int pDepth) const
    {
        uint64_t lScaledX=uint64_t(int64_t(std::floor(pV.x()/pResolution))+cZero);
        if(lScaledX>cMaxValue) return cInvalid;
        uint64_t lScaledY=uint64_t(int64_t(std::floor(pV.y()/pResolution))+cZero);
        if(lScaledY>cMaxValue) return cInvalid;
        uint64_t lScaledZ=uint64_t(int64_t(std::floor(pV.z()/pResolution))+cZero);
        if(lScaledZ>cMaxValue) return cInvalid;

        return (lScaledX+(lScaledY<<cMaxDepth)+(lScaledZ<<2*cMaxDepth))&Mask(pDepth);
    }

    int64_t CalcX() const
    {
        return int64_t(mValue&cMask)-cZero;
    }

    int64_t CalcY() const
    {
        return int64_t((mValue>>cMaxDepth)&cMask)-cZero;
    }

    int64_t CalcZ() const
    {
        return int64_t(mValue>>2*cMaxDepth)-cZero;
    }
    
    COcTreeDefaultKey(uint64_t pValue)
        :   mValue(pValue)
    {}
        
public:
    COcTreeDefaultKey()
        :   mValue(cInvalid)
    {}

    COcTreeDefaultKey(const mat::CVec3f &pV,float pResolution)
        :   mValue(CalcKey(pV,pResolution))
    {}

    COcTreeDefaultKey(const mat::CVec3f &pV,float pResolution,int pDepth)
        :   mValue(CalcKey(pV,pResolution,pDepth))
    {}

    bool Valid() const   {   return mValue!=cInvalid;    }

    uint64_t Hash() const    {   return mValue;  }
    
    ///returns the position of the key at a certain depth (the depth of the node)

    ///pDepth is the depth of the parent
    uint64_t ChildPos(int pDepth) const
    {
        int lShift=cMaxDepth-(pDepth+1);
        return ((mValue>>lShift)&1)+((mValue>>(cMaxDepth-1+lShift))&2)+((mValue>>(2*cMaxDepth-2+lShift))&4);
    }

    ///pDepth is the depth of this node, not the child
    COcTreeDefaultKey Child(uint64_t pChild,int pDepth) const
    {
        int lShift=cMaxDepth-(pDepth+1);
        return COcTreeDefaultKey(mValue+((pChild&1)<<lShift)+((pChild&2)<<(cMaxDepth-1+lShift))+
                                        ((pChild&4)<<(2*cMaxDepth-2+lShift)));
    }

    ///pDepth is the depth of this node, not the parent
    COcTreeDefaultKey Parent(int pDepth) const
    {
        return COcTreeDefaultKey(mValue&~((1+(uint64_t(1)<<cMaxDepth)+(uint64_t(1)<<2*cMaxDepth))<<(cMaxDepth-pDepth)));
    }

    COcTreeDefaultKey AncestorAt(int pDepth) const
    {
        return COcTreeDefaultKey(mValue&Mask(pDepth));
    }
    
    ///pDepth is the depth of the node
    mat::CVec3f Coords(float pResolution,int pDepth=cMaxDepth) const
    {
        float lCorrection=0.5f*(uint64_t(1)<<(cMaxDepth-pDepth));
        
        return mat::CVec3f({(CalcX()+lCorrection)*pResolution,(CalcY()+lCorrection)*pResolution,
                         (CalcZ()+lCorrection)*pResolution});
    }

    float X(float pResolution,int pDepth) const
    {
        float lCorrection=0.5f*(uint64_t(1)<<(cMaxDepth-pDepth));

        return (CalcX()+lCorrection)*pResolution;
    }

    float Y(float pResolution,int pDepth) const
    {
        float lCorrection=0.5f*(uint64_t(1)<<(cMaxDepth-pDepth));

        return (CalcY()+lCorrection)*pResolution;
    }

    float Z(float pResolution,int pDepth) const
    {
        float lCorrection=0.5f*(uint64_t(1)<<(cMaxDepth-pDepth));

        return (CalcZ()+lCorrection)*pResolution;
    }

    void IncX(int64_t pInc) //increments in terms of key
    {
        mValue+=pInc;
    }

    void IncY(int64_t pInc) //increments in terms of key
    {
        mValue+=pInc*(int64_t(1)<<cMaxDepth);
    }

    void IncZ(int64_t pInc) //increments in terms of key
    {
        mValue+=pInc*(int64_t(1)<<2*cMaxDepth);
    }

    void Print(io::CTextWriter &pWriter) const
    {
        for(int i=3*cMaxDepth-1;i>=0;i--)
        {
            pWriter << ((mValue&(uint64_t(1)<<i))?"1":"0");
            if(i%cMaxDepth==0&&i)
                pWriter << "-";
        }
    }

    static COcTreeDefaultKey Root()
    {
        return COcTreeDefaultKey(0);
    }

    bool operator==(const COcTreeDefaultKey &pRH) const
    {
        return mValue==pRH.mValue;
    }
    
private:
    uint64_t mValue;
};

inline io::CTextWriter &operator<<(io::CTextWriter &pWriter,const COcTreeDefaultKey &pKey)
{
    pKey.Print(pWriter);
    return pWriter;
}

/*namespace cvi*/ } /*namespace buola*/ }

namespace std {
    template<>
    struct hash<buola::cvi::COcTreeDefaultKey> : public std::unary_function<buola::cvi::COcTreeDefaultKey,std::size_t>
    {
        std::size_t operator()(const buola::cvi::COcTreeDefaultKey &pKey) const
        {
            return pKey.Hash();
        }
    };
}

#endif
